#include "WPILib.h"
#include "NetworkTables/NetworkTable.h"
#include "SmartDashboard.h"
#include "DriverStation.h"


/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 

class RobotDemo : public SimpleRobot
{
	//RobotDrive myRobot; // robot drive system
	//Joystick stick; // only joystick
	
public:
	RobotDemo(void)
	    //:
		//myRobot(1, 2),	// these must be initialized in the same order
		//stick(1)		// as they are declared above.
	{
		//myRobot.SetExpiration(0.1);
		NetworkTable::GetTable("robotMovement")->PutNumber("angle",0.0);
		NetworkTable::GetTable("robotMovement")->PutNumber("currAngle",10.0);
		NetworkTable::GetTable("robotMovement")->PutBoolean("tableRead",false);
		NetworkTable::GetTable("robotMovement")->PutBoolean("tableUpdated",false);
		NetworkTable::GetTable("robotMovement")->PutBoolean("tableInUse",false);
		NetworkTable::GetTable("robotMovement")->PutBoolean("adjustEnabled",false);
		NetworkTable::GetTable("robotMovement")->PutBoolean("foward",false);
		NetworkTable::GetTable("robotMovement")->PutBoolean("back",false);
		NetworkTable::GetTable("robotMovement")->PutBoolean("left",false);
		NetworkTable::GetTable("robotMovement")->PutBoolean("right",false);
		NetworkTable::GetTable("robotMovement")->PutBoolean("strafeLeft",false);
		NetworkTable::GetTable("robotMovement")->PutBoolean("strafeRight",false);
		NetworkTable::GetTable("robotMovement")->PutString("currMode","None");
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		NetworkTable::GetTable("robotMovement")->PutString("currMode","Autonomous");
		printf("Robot is now in: Autonomous mode.\n");
		while(IsAutonomous() && IsEnabled()) {
			if(NetworkTable::GetTable("robotMovement")->GetBoolean("adjustEnabled")) {
				if(!NetworkTable::GetTable("robotMovement")->GetBoolean("tableRead")) {
					printf("Adjustment authorized!\n");
				}
				handleRobotAdjustment();
				fireFrisbee();
			}
			Wait(1);
		}
		//myRobot.SetSafetyEnabled(false);
		//myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
		//Wait(2.0); 				//    for 2 seconds
		//myRobot.Drive(0.0, 0.0); 	// stop robot
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		NetworkTable::GetTable("robotMovement")->PutString("currMode","Teleoperated");		
		printf("Robot is now in: Teleoperated mode.\n");
		//myRobot.SetSafetyEnabled(true);
		
		// First use: STATUS does not exist, create it. Set the boolean value
		// Robot_Enabled (create if first time) to FALSE
		//printf("This print statement happened\n");
		while (IsOperatorControl() && IsEnabled())
		{
			if(NetworkTable::GetTable("robotMovement")->GetBoolean("adjustEnabled")) {
				if(!NetworkTable::GetTable("robotMovement")->GetBoolean("tableRead")) {
					printf("Adjustment authorized!\n");
				}
				handleRobotAdjustment();
				fireFrisbee();
			}
			Wait(1);
			//myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			//Wait(0.5);				// wait for a motor update time
			//while (IsEnabled()) m_ds->WaitForData(); // Wait for a data packet
		}
	}
	// Checks if table isn't being used currently and has been updated
	// and then begins adjusting robot until all movements are false and 
	// curAngle == angle.  Frisbee is then fired.
	void handleRobotAdjustment() {
		bool adjust = checkAdjustment();
		printf("Handling Robot Adjustment!\n");
		if(NetworkTable::GetTable("robotMovement")->GetBoolean("tableUpdated")) {
			printf("Table has been updated, wee!\n");
			while(adjust) {
				if((!(NetworkTable::GetTable("robotMovement")->GetBoolean("tableInUse"))) &&
					 NetworkTable::GetTable("robotMovement")->GetBoolean("tableUpdated")) {
					// Lock table values
					printf("Table not in use, locking!\n");
					NetworkTable::GetTable("robotMovement")->PutBoolean("tableInUse",true);
					printf("Removing updated status!\n");
					NetworkTable::GetTable("robotMovement")->PutBoolean("tableUpdated",false);
					strafeLeft();
					strafeRight();
					left();
					right();
					foward();
					back();
					updateLauncherAngle(NetworkTable::GetTable("robotMovement")->GetNumber("angle"));
					printf("Indicating table has been read!\n");
					NetworkTable::GetTable("robotMovement")->PutBoolean("tableRead",true);
					// Release lock on table
					printf("Releasing lock on table!\n");
					NetworkTable::GetTable("robotMovement")->PutBoolean("tableInUse",false);
					adjust = checkAdjustment();
				}else if(NetworkTable::GetTable("robotMovement")->GetBoolean("tableInUse")){
					printf("Table is locked, awaiting updates =)\n");
					Wait(1);
				}
				Wait(0.2);
			}
		}
	}
	bool checkAdjustment() {
		return(NetworkTable::GetTable("robotMovement")->GetBoolean("strafeLeft") ||
			   NetworkTable::GetTable("robotMovement")->GetBoolean("strafeRight") ||
			   NetworkTable::GetTable("robotMovement")->GetBoolean("left") ||
			   NetworkTable::GetTable("robotMovement")->GetBoolean("right") ||
			   NetworkTable::GetTable("robotMovement")->GetBoolean("foward") ||
			   NetworkTable::GetTable("robotMovement")->GetBoolean("back") ||
			   (NetworkTable::GetTable("robotMovement")->GetNumber("angle") != 45.0));
	}
	void strafeLeft() {
		if(NetworkTable::GetTable("robotMovement")->GetBoolean("strafeLeft")) {
			//move a little
			printf("Burping Cims and strafing Left =D\n");
		}
	}
	void strafeRight() {
		if(NetworkTable::GetTable("robotMovement")->GetBoolean("strafeRight")) {
			//move a little
			printf("Burping Cims and strafing Right =D\n");
		}
	}
	void foward() {
		if(NetworkTable::GetTable("robotMovement")->GetBoolean("foward")) {
			//move a little
			printf("Burping Cims and moving Foward =D\n");
		}
	}
	void back() {
		if(NetworkTable::GetTable("robotMovement")->GetBoolean("back")) {
			//move a little
			printf("Burping Cims and moving Back =D\n");
		}
	}
	void left() {
		if(NetworkTable::GetTable("robotMovement")->GetBoolean("left")) {
			//move a little
			printf("Burping Cims and turning Left =D\n");
		}
	}
	void right() {
		if(NetworkTable::GetTable("robotMovement")->GetBoolean("right")) {
			//move a little
			printf("Burping Cims and turning Right =D\n");
		}		
	}
	void updateLauncherAngle(double angle) {
		double angleUpdate = angle;
		while(angleUpdate > getCurrLauncherAngle()) {
			//move launcher down
			printf("Adjusting Launcher down to proper angle, current angle: %f\n",angleUpdate);
			angleUpdate--;			
		}
		while(angleUpdate < getCurrLauncherAngle()) {
			//move launcher up
			printf("Adjusting Launcher up to proper angle, current angle: %f\n",angleUpdate);
			angleUpdate++;
		}
	}
	double getCurrLauncherAngle() {
		double angle = 45.0;
		return angle;
	}
	void fireFrisbee() {
		if(NetworkTable::GetTable("robotMovement")->GetBoolean("adjustEnabled") && 
		   NetworkTable::GetTable("robotMovement")->GetBoolean("tableRead")) {
			printf("Frisbee(s) fired, death from below! >:D\n");
		}
	}
	/**
	 * Runs during test mode
	 */
	void Test() {
		NetworkTable::GetTable("robotMovement")->PutString("currMode","Test");
		printf("Robot is now in: Test mode.\n");
	}
};

START_ROBOT_CLASS(RobotDemo);

